package tools;

/**Fast contains static helper methods useful to many other classes
 * @author Antoine Lennartz*/
public class Fast {
	
	public static final double DEG_TO_RAD=Math.PI/180.0;
	
	/**
	 * @param angle - an angle in radians
	 * 
	 * @return an angle in degrees between 0 and 2PI*/
	public static double fixAngle(double angle) {
			angle=angle%(2*Math.PI);
			if(angle<0) angle+=(2*Math.PI);
			return angle;
	}
	
	/**@param angle -  an angle in radians
	 * @return an angle between -PI and PI*/
	public static double fixAngle2(double angle){
		angle = fixAngle(angle);
		if(angle>Math.PI) angle-=(2*Math.PI);
		return angle;
		
	}
	/**returns magnitude of 2d vector
	 * @param vect - the vector*/
	public static double magnitude(double[] vect){
		return Math.sqrt(vect[0]*vect[0] + vect[1]*vect[1]);
	}
	
	public static int round(double d){
		if(d-((int) d)<0.5){
			return (int)d;
		}else return (int)d+1;
	}
	/**@param theta - an angle in degrees
	 * @return the angle in radians*/
	public static double toRad(double theta){
		 return Math.PI*theta/180;
	 }
	
	/**
	 * @param x - x position on grid
	 * @param y - y position on grid
	 * @return theta: angle in degrees from origin to point(x,y) with angle defined in compass fashion*/
	/*public static double atan3(double x, double y){
		double theta=Math.atan(x/y)*180/Math.PI;
		Math.atan2(arg0, arg1);
		if(x>0){
			if(y<0){
				theta+=180;
			}
		}else{
			theta+=360;
			if(y<0){
				theta-=180;
			}
		}
		return theta;
	}*/

	/**@param v - double
	 * @return 1 if v is>=0, -1 if v<0*/
	public static double signOf(double v){
		if(v==0) return 1;
		return (v/Math.abs(v));
	}
	
	/**works if points are position vectors from odometer as pos[2] is ignored
	 * @param pointA - [x,y] coordinates of point
	 * @param pointB - [x,y] coordinates of point
	 * @return distance from pointA to pointB*/
	public static double distance(double[] pointA, double[] pointB){
		return Math.sqrt( (pointA[0]-pointB[0])*(pointA[0]-pointB[0]) + (pointA[1]-pointB[1])*(pointA[1]-pointB[1]) );
	}
	/**ok... its only bubblesort dont get to excited
	 * @param array - the array to be sorted*/
	public static void sort(double[] array){
		double tmp;
		for(int j=array.length-1; j>0; j--){
			for(int i=0; i<j; i++){
				if(array[i]>array[i+1]){
					tmp=array[i+1];
					array[i+1]=array[i];
					array[i]=tmp;
				}
			}
		}
	}

	/**nearest grid Crossing coordinates, preserves heading
	 * @param cPos - position close to the grid crossing*/
	public static double[] nearestCross(double[] cPos){
		return new double[]{Math.round(cPos[0]/30.48)*30.48, Math.round(cPos[1]/30.48)*30.48, cPos[2]};
	}
}
